clear; clc; close all;

% 地图参数
map_size = 20;      % 地图尺寸(米)
resolution = 0.5;   % 分辨率(米/格)
origin = [0, 0];    % 地图原点

% 创建空地图 (无任何障碍物)
map = zeros(ceil(map_size/resolution), ceil(map_size/resolution));

% 添加障碍物 (修改处1：添加障碍物)
map = add_obstacles(map, resolution, origin);

% 设置起点和终点
start = [2, 2];     % 起点(世界坐标)
goal = [18, 18];    % 终点(世界坐标)

%% A*全局路径规划
fprintf('正在执行A*全局路径规划...\n');
astar = AStarPlanner(map, resolution, origin);
global_path = astar.plan(start, goal);
fprintf('全局路径规划完成，路径长度: %d个点\n', size(global_path, 1));

%% DWA局部路径跟踪
fprintf('开始DWA局部导航...\n');
dwa = SimpleDWA();
initial_heading = atan2(goal(2)-start(2), goal(1)-start(1)); % 初始朝向目标
robot_pose = [start(1), start(2), initial_heading, 0, 0]; % [x,y,θ,v,w]

% 可视化初始化
fig = figure;
set(fig, 'Position', [100, 100, 1000, 800]);
hold on; axis equal; grid on;
axis([0 map_size 0 map_size]);
title('A*+DWA整合导航 - 带障碍物版本');
xlabel('X (m)'); ylabel('Y (m)');

% 绘制障碍物 (修改处2：可视化障碍物)
draw_obstacles(map, resolution, origin);

% 绘制全局路径和起终点
plot(global_path(:,1), global_path(:,2), 'b-', 'LineWidth', 1.5, 'DisplayName', '全局路径');
plot(start(1), start(2), 'go', 'MarkerSize', 10, 'MarkerFaceColor', 'g', 'DisplayName', '起点');
plot(goal(1), goal(2), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r', 'DisplayName', '终点');

% 初始化绘图对象
robot_plot = plot(robot_pose(1), robot_pose(2), 'bo', 'MarkerSize', 12, 'MarkerFaceColor', 'b', 'DisplayName', '机器人');
quiver_plot = quiver(robot_pose(1), robot_pose(2), 0.5*cos(robot_pose(3)), 0.5*sin(robot_pose(3)), ...
                   'b', 'LineWidth', 1.5, 'MaxHeadSize', 1, 'DisplayName', '朝向');
path_plot = plot(robot_pose(1), robot_pose(2), 'm-', 'LineWidth', 1.5, 'DisplayName', '实际路径');

%% 主控制循环
current_goal_idx = 1;  % 当前目标点索引
path_history = robot_pose(1:2); % 路径历史记录
step_count = 0;
max_steps = 400; % 最大步数限制

% 添加调试变量
last_v = 0;
last_w = 0;

while step_count < max_steps
    step_count = step_count + 1;
    
    % 获取当前局部目标点
    [current_goal_idx, local_goal] = get_local_goal(robot_pose, global_path, current_goal_idx);
   
    % 获取局部障碍物 
    obstacles = get_local_obstacles(robot_pose, map, resolution, origin, 5.0); % 5米检测范围
    
    % DWA规划
    [new_v, new_w, best_traj] = dwa.plan(robot_pose, local_goal, obstacles);
    
    % 确保新指令被应用
    % if ~isempty(new_v) && ~isempty(new_w)
        v = new_v;
        w = new_w;
    % else
    %     % 使用上次指令作为后备
    %     v = last_v;
    %     w = last_w;
    %     fprintf('警告：DWA未返回有效指令，使用上次值 v=%.2f, w=%.2f\n', v, w);
    % end
    
    % 更新机器人位姿
    robot_pose = update_pose(robot_pose, v, w, dwa.dt);
    path_history = [path_history; robot_pose(1:2)];
    
    % 存储当前指令
    last_v = v;
    last_w = w;
    
    % 更新可视化
    update_visualization(robot_plot, quiver_plot, path_plot, robot_pose, path_history);
    
    % 绘制局部目标和预测轨迹
    plot(local_goal(1), local_goal(2), 'cx', 'MarkerSize', 8, 'LineWidth', 1.5, 'HandleVisibility', 'off');
    
    % 绘制预测轨迹
    if ~isempty(best_traj)
        plot(best_traj(:,1), best_traj(:,2), 'c:', 'LineWidth', 0.8, 'HandleVisibility', 'off');
    end
    
    % 检查是否到达最终目标
    dist_to_goal = norm(robot_pose(1:2) - goal);
    if dist_to_goal < 0.5
        fprintf('成功到达目标点! 步数: %d\n', step_count);
        break;
    end
    
    % 控制循环速度
    pause(0.05);
    
    % 显示详细的指令信息
    fprintf('Step %d: 位置[%.2f, %.2f], 朝向%.2f rad\n', step_count, robot_pose(1), robot_pose(2), robot_pose(3));
    fprintf('  接收指令: v=%.2f m/s, w=%.2f rad/s\n', v, w);
    fprintf('  实际应用: v=%.2f m/s, w=%.2f rad/s\n', robot_pose(4), robot_pose(5));
    
    % 检查指令一致性
    if abs(v - robot_pose(4)) > 0.01 || abs(w - robot_pose(5)) > 0.01
        fprintf('  警告：指令与应用值不一致！\n');
    end
end

% 绘制最终结果
plot(global_path(:,1), global_path(:,2), 'b-', 'LineWidth', 1.5);
plot(path_history(:,1), path_history(:,2), 'm-', 'LineWidth', 1.5);
legend('show', 'Location', 'best');
fprintf('导航结束\n');

%% ========== 障碍物相关函数 ==========
function map = add_obstacles(map, resolution, origin)
    % 添加几个简单的障碍物
    [rows, cols] = size(map);
    rect_poses = [6,8; 14,9];

    for i = 1:2
        % 添加一个矩形障碍物
        rect_pos = [rect_poses(i,1), rect_poses(i,2)]; % 世界坐标
        rect_size = [3, 3]; % 宽度和高度(米)
        rect_idx = world_to_map_idx(rect_pos, resolution, origin);
        rect_size_idx = round(rect_size ./ resolution);
    
        % 确保不越界
        row_start = max(1, rect_idx(1)-rect_size_idx(2)/2);
        row_end = min(rows, rect_idx(1)+rect_size_idx(2)/2);
        col_start = max(1, rect_idx(2)-rect_size_idx(1)/2);
        col_end = min(cols, rect_idx(2)+rect_size_idx(1)/2);
        
        map(row_start:row_end, col_start:col_end) = 1;
    end
    
    % 添加几个圆形障碍物
    circles = [12, 5, 1.5;  % x,y,radius
               5, 12, 1.5;
               15, 15, 2];
    
    for i = 1:size(circles, 1)
        circle_pos = circles(i,1:2);
        radius = circles(i,3);
        radius_idx = round(radius / resolution);
        
        circle_idx = world_to_map_idx(circle_pos, resolution, origin);
        
        % 在圆形区域内设置障碍物
        for r = max(1, circle_idx(1)-radius_idx):min(rows, circle_idx(1)+radius_idx)
            for c = max(1, circle_idx(2)-radius_idx):min(cols, circle_idx(2)+radius_idx)
                dist = norm([r,c] - circle_idx);
                if dist <= radius_idx
                    map(r,c) = 1;
                end
            end
        end
    end
end

function draw_obstacles(map, resolution, origin)
    % 绘制障碍物
    [rows, cols] = size(map);
    for r = 1:rows
        for c = 1:cols
            if map(r,c) == 1
                pos = map_idx_to_world([r,c], resolution, origin);
                rectangle('Position', [pos(1)-resolution/2, pos(2)-resolution/2, resolution, resolution], ...
                          'FaceColor', [0.3 0.3 0.3], 'EdgeColor', 'none');
            end
        end
    end
end

function obstacles = get_local_obstacles(pose, map, resolution, origin, detection_range)
    % 获取机器人周围的局部障碍物
    obstacles = [];
    
    % 转换为地图索引
    robot_idx = world_to_map_idx(pose(1:2), resolution, origin);
    range_idx = round(detection_range / resolution);
    
    [rows, cols] = size(map);
    
    % 搜索局部区域
    r_start = max(1, robot_idx(1)-range_idx);
    r_end = min(rows, robot_idx(1)+range_idx);
    c_start = max(1, robot_idx(2)-range_idx);
    c_end = min(cols, robot_idx(2)+range_idx);
    
    for r = r_start:r_end
        for c = c_start:c_end
            if map(r,c) == 1 % 是障碍物
                pos = map_idx_to_world([r,c], resolution, origin);
                obstacles = [obstacles; pos];
            end
        end
    end
end

function idx = world_to_map_idx(world_pos, resolution, origin)
    % 世界坐标 → 地图索引
    x = world_pos(1) - origin(1);
    y = world_pos(2) - origin(2);
    col = floor(x / resolution) + 1;
    row = floor(y / resolution) + 1;
    idx = [row, col];
end

function world_pos = map_idx_to_world(idx, resolution, origin)
    % 地图索引 → 世界坐标
    x = origin(1) + (idx(2)-1)*resolution + resolution/2;
    y = origin(2) + (idx(1)-1)*resolution + resolution/2;
    world_pos = [x, y];
end

function [current_goal_idx, local_goal] = get_local_goal(pose, global_path, current_idx)
    lookahead_dist = 3.0; % 增加前视距离
    
    % 确保current_idx不超过路径长度
    current_idx = min(current_idx, size(global_path, 1));
    
    % 从当前索引开始搜索
    for i = current_idx:size(global_path, 1)
        dist = norm(global_path(i,:) - pose(1:2));
        if dist >= lookahead_dist
            current_goal_idx = i;
            local_goal = global_path(i,:);
            return;
        end
    end
    
    % 如果所有点都在前视距离内，选择最终目标
    current_goal_idx = size(global_path, 1);
    local_goal = global_path(end,:);
end

function pose = update_pose(pose, v, w, dt)
    % 更新机器人位姿
    pose(3) = pose(3) + w * dt;           % 更新角度
    pose(3) = wrapToPi(pose(3));          % 规范化角度到[-π, π]
    
    % 使用新角度更新位置
    dx = v * cos(pose(3)) * dt;
    dy = v * sin(pose(3)) * dt;
    pose(1) = pose(1) + dx;
    pose(2) = pose(2) + dy;
    
    % 存储速度指令
    pose(4) = v;  % 线速度
    pose(5) = w;  % 角速度
end

function update_visualization(robot_plot, quiver_plot, path_plot, pose, path_history)
    set(robot_plot, 'XData', pose(1), 'YData', pose(2));
    set(quiver_plot, 'XData', pose(1), 'YData', pose(2), ...
        'UData', 0.5*cos(pose(3)), 'VData', 0.5*sin(pose(3)));
    set(path_plot, 'XData', path_history(:,1), 'YData', path_history(:,2));
    drawnow limitrate;
end